#include <ros/ros.h>
#include <thread>


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

#include "tcp.h"
#include "tcp2485.h"
#include <speed_msg/ReadSpeed.h>
#include <speed_msg/WriteSpeed.h>
#include <thread>
#include "util.h"



#define PORT 8899
#define IP "192.168.0.102"

ros::Publisher pub;



void read_cb(struct bufferevent *bev, void *arg)
{
	unsigned char buf[1024] = {0};
	int size = bufferevent_read(bev, buf, sizeof(buf));
	if(size <2)
	{
	    return;
	}

	string strRecv;
	for (int i = 0; i < size; i++)
	{
		strRecv += unsignedCharToHex(buf[i]);
	}

	printf("recv:%s\n", strRecv.c_str());
	std::thread::id this_id = std::this_thread::get_id();

	if (buf[0] == 0x01 && buf[1] == 0x03) // 读取数据的响应
	{
		if (size != 13)
		{
			return;
		}

		uint16_t rspCrc = CRC_Check(buf, 11);
		if (buf[11] != (unsigned char)(rspCrc >> 8) || buf[12] != (unsigned char)(rspCrc << 8 >> 8))
		{
			printf("getWriteParam rsp crc err\n");
			return;
		}

		speed_msg::ReadSpeed msg;
		msg.bFatalStop = buf[3] & 1;
		msg.bMainEngineRunState = buf[4] & 1;
		msg.leftSpeed = buf[7] * 256 + buf[8];
		msg.rightSpeed = buf[9] * 256 + buf[10];

		msg.ip = getPeerIP(bev);
		
		pub.publish(msg);
		return;
	}

	if (buf[0] == 0x01 && buf[1] == 0x10) // 写数据的响应
	{

	}
}

void read485(const ros::TimerEvent &even)
{
	// ROS_INFO("timer callback triggerd");
	MachineReadParam param;
	getReadParam(IP, param);
	// ROS_INFO("fatalWarn:%d, leftSpeed:%d,rightSpeed:%d",param.getFatalWarn(), param.getLeftSpeed(), param.getRightSpeed());
}

void connectToServer()
{
	connect(IP, PORT, read_cb);
}


void write_speed(speed_msg::WriteSpeed msg){
	unsigned char arr[15] ={0x01, 0x10, 0x00,0x05,0x00,0x03,0x06};

	arr[7] = (arr[7] | 1);
	//lightWarn ?arr[7] = (arr[7] | (1<< 1)) : arr[7] =(arr[7] & ~(1 <<1));
	msg.bFatalStop ? arr[7] = (arr[7] | (1<< 2)) : arr[7] =(arr[7] & ~(1 <<2));
	arr[8] = msg.direction;

	 uint16_t leftSpeed = msg.leftSpeed;
    arr[9] = leftSpeed >> 8;
    arr[10] = leftSpeed << 8 >>8;

	uint16_t rightSpeed = msg.rightSpeed;
    arr[11] = rightSpeed >> 8;
    arr[12] = rightSpeed << 8 >>8;

    uint16_t crc =CRC_Check(arr,13);
    arr[13] = crc >> 8;
    arr[14] = crc << 8 >>8;

	string recv;//此参数目前没有用 
	wirteToServer(msg.ip, arr, 15,recv);
}

int main(int argc, char *argv[])
{
	// 连接tcp服务端
	std::thread t(connectToServer);
	t.detach();

	ros::init(argc, argv, "tcp2modbus_node");
	printf("Hello world\n");

	ros::NodeHandle nh;
	// 创建一个定时器，参数分别是持续时间（秒），回调函数，是否为一次性（false表示重复）

	pub = nh.advertise<speed_msg::ReadSpeed>("/tcp2modbus/read_speed", 1000); // 发布uwb数据  话题名 队列大小
	ros::Subscriber sub = nh.subscribe("/tcp2modbus/write_speed",10, write_speed);//


	std::thread::id this_id = std::this_thread::get_id();
	printf("Current thread ID: %d", this_id);

	ros::Timer timer = nh.createTimer(ros::Duration(1.0), read485, false);
	ros::Rate loop_rate(10); // 定义发布话题的频率，1秒10次

	// todo 将读取到的数据发送出去
	while (ros::ok())
	{
		ros::spinOnce();
		loop_rate.sleep(); // 根据前面的频率自动休眠
	}
	return 0;
}
